#include "utils.h"
#include "preprocesamiento.h"
#include <iostream>

void cerrarTodo(){
	destroyAllWindows();
}
//Angulo en sexagesimal
double angleBetween( cv::Point pt1, cv::Point pt2, cv::Point pt0 )
{
	double dx1 = pt1.x - pt0.x;
	double dy1 = pt1.y - pt0.y;
	double dx2 = pt2.x - pt0.x;
	double dy2 = pt2.y - pt0.y;
	return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

Point2f computeIntersect(Vec4i a, Vec4i b)  
{  
	int x1 = a[0], y1 = a[1], x2 = a[2], y2 = a[3];  
	int x3 = b[0], y3 = b[1], x4 = b[2], y4 = b[3];  
	if (x1 == 0 && x3 == 0) return Point2f(-1,-1); 
	if (y1 == 0 && y3 == 0) return Point2f(-1,-1);
	// Solo se computaran las intersecciones entre lineas horizontales y verticales
	if (float d = ((float)(x1-x2) * (y3-y4)) - ((y1-y2) * (x3-x4)))  
	{  
		Point2f pt;  
		pt.x = ((x1*y2 - y1*x2) * (x3-x4) - (x1-x2) * (x3*y4 - y3*x4)) / d;  
		pt.y = ((x1*y2 - y1*x2) * (y3-y4) - (y1-y2) * (x3*y4 - y3*x4)) / d; 
		/*
		//-20 is a threshold, the POI can be off by at most 20 pixels
		if(pt.x<min(x1,x2)-20||pt.x>max(x1,x2)+20||pt.y<min(y1,y2)-20||pt.y>max(y1,y2)+20){  
		return Point2f(-1,-1);  
		}  
		if(pt.x<min(x3,x4)-20||pt.x>max(x3,x4)+20||pt.y<min(y3,y4)-20||pt.y>max(y3,y4)+20){  
		return Point2f(-1,-1);  
		}  */
		return pt;  
	}  
	else  
		return Point2f(-1, -1);  
} 

bool comparator(Point2f a,Point2f b){  
	return a.x<b.x;  
}  

bool comparatorRect(Rect a,Rect b){  
	return a.x<b.x;  
}  
void sortCorners(vector<Point2f>& corners, Point2f center)  
{  
	vector<Point2f> top, bot;  
	for (size_t i = 0; i < corners.size(); i++)  
	{  
		if (corners[i].y < center.y)  
			top.push_back(corners[i]);  
		else  
			bot.push_back(corners[i]);  
	}  
	sort(top.begin(),top.end(),comparator);  
	sort(bot.begin(),bot.end(),comparator);  
	Point2f tl = top[0];
	Point2f tr = top[top.size()-1];
	Point2f bl = bot[0];
	Point2f br = bot[bot.size()-1];  
	corners.clear();  
	corners.push_back(tl);  
	corners.push_back(tr);  
	corners.push_back(br);  
	corners.push_back(bl);  
}  

int distanceBetween( cv::Point pt1, cv::Point pt2 ){
	int dx = pt1.x - pt2.x;
	int dy = pt1.y - pt2.y;
	double distance = sqrt(dx*dx + dy*dy);
	return (int)distance;
}

double distanceBetweenF( cv::Point pt1, cv::Point pt2 ){
	int dx = pt1.x - pt2.x;
	int dy = pt1.y - pt2.y;
	double distance = sqrt(dx*dx + dy*dy);
	return distance;
}

bool cumpleRatio(vector<Point2f> poligono,double valor,double errorNeg,double errorPos){
	double ancho = distanceBetweenF(poligono[0],poligono[1]);
	double alto = distanceBetweenF(poligono[0],poligono[3]);
	double ratioWH = ancho/alto;
	return ((ratioWH <= valor + errorPos) && (ratioWH >= valor - errorNeg));
}

bool cumpleRatio(int w, int h,double valor, double errorNeg,double errorPos){
	double ancho = w;
	double alto = h;
	double ratioWH = ancho/alto;
	return ((ratioWH <= valor + errorPos) && (ratioWH >= valor - errorNeg));
}
bool esParalelogramo(vector<Point2f> poligono,double error){
	double anchoSuperior = distanceBetweenF(poligono[0],poligono[1]);
	double anchoInferior = distanceBetweenF(poligono[2],poligono[3]);
	double altoDerecho = distanceBetweenF(poligono[1],poligono[2]);
	double altoIzquierdo = distanceBetweenF(poligono[3],poligono[0]);

	double deltaAncho = abs(anchoSuperior - anchoInferior);
	double deltaAlto = abs(altoDerecho - altoIzquierdo);
	return ((deltaAncho < error) && (deltaAlto < error));
}

int * calcularHistograma(unsigned char* img_data, int w, int h){
	int * histograma = (int*)calloc(256,sizeof(int));
	for (size_t i = 0; i < w*h; i++){
		histograma[img_data[i]]++;
	}
	return histograma;
}
int * calcularSumaCortesHorizontales(Mat img){
	unsigned char* img_data = img.data;
	int w = img.cols;
	int h = img.rows;
	int * histograma = (int*)calloc(h,sizeof(int));
	for (size_t i = 0; i < h; i++){
		for (size_t j = 0; j < w; j++){
			histograma[i]+= img_data[i*w+j];
		}
	}
	return histograma;
}
int * calcularSumaCortesVerticales(Mat img){
	unsigned char* img_data = img.data;
	int w = img.cols;
	int h = img.rows;
	int * histograma = (int*)calloc(w,sizeof(int));
	for (size_t j = 0; j < w; j++){
		for (size_t i = 0; i < h; i++){
			int pixel = img_data[i*w+j];
			histograma[j]+= (pixel == 255 ? 0 : 1);
		}
	}
	return histograma;
}

Mat perspectiva(Mat src, vector<Point2f> corners,int rows,int cols,int k){
	Mat aux = src.clone();
	// Define the destination image
	Mat quad = Mat::zeros(rows, cols, CV_8UC3);

	// Corners of the destination image
	vector<Point2f> quad_pts;
	quad_pts.push_back(Point2f(0, 0));
	quad_pts.push_back(Point2f(quad.cols, 0));
	quad_pts.push_back(Point2f(quad.cols, quad.rows));
	quad_pts.push_back(Point2f(0, quad.rows));
	Mat transmtx = getPerspectiveTransform(corners, quad_pts);
	/*
	// Get transformation matrix
	for (int i=0; i<4; i++){
		printf("x=%.3f\ty=%.3f\n",corners[i].x,corners[i].y);
	}
	for (int i=0; i<3; i++){
		for(int j=0; j<3;j++){
			printf("%.3f\t",transmtx.at<double>(i*3+j));
		}
		cout << endl;
	}
	cout << endl;*/
	// Apply perspective transformation
	warpPerspective(aux, quad, transmtx, quad.size());
	return quad;
}

int horizontalCrosscut(Mat src,int row){
	int width = src.cols;
	unsigned char * data = src.data;
	int flagZona = 0;
	int pixel;
	int inicio = 0,fin = width-1;
	int nObjetos = 0;
	for (int i = 0; i < width; i++){
		pixel = data[row*width + i];
		if (pixel == 0){
			inicio = i;
			break;
		}
	}
	for (int i = width - 1; i < 0; i++){
		pixel = data[row*width + i];
		if (pixel == 0){
			fin = i;
			break;
		}
	}
	for (int i = inicio; i <=fin; i++){
		pixel = data[row*width + i];
		if (pixel != flagZona){
			if (flagZona == 0)
				nObjetos++;
			flagZona = pixel;
		}
	}
	return nObjetos;
}

int * derivadaHist(int* funcion,int length){
	int * derivada = (int*) calloc(length,sizeof(int));
	for (int i = 0; i < length - 1; i++){
		derivada[i] = funcion[i+1] - funcion[i];
	}
	derivada[length - 1] = funcion[length - 1];
	return derivada;
}

int * hallarTransiciones(int* funcion,int length){
	int * transiciones = (int*) calloc(length,sizeof(int));
	int puntero = funcion[0];
	for (int i = 0; i < length; i++){
		if (funcion[i] != puntero){
			if (puntero == 0)
				transiciones[i]++; //Si crece desde 0 es transicion positiva
			if (funcion[i] == 0)
				transiciones[i]--; //Si decrece a 0 es transicion negativa
		}
		puntero = funcion[i];
	}
	return transiciones;
}

int * hallarMaximosMitad(int* funcion,int length,int width){
	int * derivada = derivadaHist(funcion,length);
	int * maximosLocales = (int*) calloc(length,sizeof(int));
	int * maximos = (int*) calloc(2,sizeof(int));
	int puntero = derivada[0];
	for (int i = 0; i < length; i++){
		if (puntero * derivada[i] <= 0 && (puntero - derivada[i] != 0) ){
			maximosLocales[i]++;
		}
		puntero = derivada[i];
	}

	int * derivada2 = derivadaHist(derivada,length);
	int valMax = 0,lastMax = 0;
	int max = 0;

	for (int i=length/3; i > length/7 ; i--){
		if (maximosLocales[i]){
			lastMax = i;
		}
		if (valMax < maximosLocales[i]*funcion[i]){
			max = i;
			valMax = abs(funcion[i]);
		}
	}
	maximos[0] = max;
	valMax = 0;
	max = length/2;
	for (int i=length*2/3; i < length-1; i++){
		if (funcion[i] < width*255/10 || funcion[i] > width*255*9/10) {
			max = i - 4;
			break;
		}
		if (valMax < maximosLocales[i]*abs(funcion[i])){
			max = i;
			valMax = abs(funcion[i]);
		}
	}
	maximos[1] = max;
	free(derivada);
	return maximos;
}
void truncarHistograma(int * hist, int length,int threshold){
	for (int i = 0; i < length - 1; i++){
		if (hist[i] < threshold)
			hist[i] = 0;
	}
}


void eliminarParalelas(vector<Vec4i> lines,vector<Vec4i> &lines2){
	int * blacklist = (int*) calloc(lines.size(),sizeof(int));
	if (lines.size()){
		for( int i = 0; i < lines.size(); i++ )
		{
			Vec4i segmentoI = lines[i];
			if (blacklist[i]) 
				continue;
			for( size_t j = i+1; j < lines.size(); j++ ){
				if (blacklist[j]) 
					continue;
				Vec4i segmentoJ = lines[j];
				int xi1 = segmentoI[0],yi1 = segmentoI[1],xi2 = segmentoI[2],yi2 = segmentoI[3];
				int xj1 = segmentoJ[0],yj1 = segmentoJ[1],xj2 = segmentoJ[2],yj2 = segmentoJ[3];
				if (abs(xi1 - xj1) <= 15 && abs(xi2 - xj2)<=15 && abs(yi1 - yj1) <= 15 && abs(yi2 - yj2)<=15)
					blacklist[j] = 1;
			}
			lines2.push_back(lines[i]);
		}
	}
	free(blacklist);
}

void agregarLineas(vector<vector<vector<cv::Point> > > &vectorLineas,vector<Vec4i>lines){
	vector<vector<cv::Point> >lineas;
	for (int i = 0; i < lines.size(); i++){
		vector<cv::Point> linea;
		linea.push_back(cv::Point(lines[i][0],lines[i][1]));
		linea.push_back(cv::Point(lines[i][2],lines[i][3]));
		lineas.push_back(linea);
	}
	vectorLineas.push_back(lineas);
}
void convertirPoint2fToPoint(vector<Point2f> v1,vector<Point> &v2){
	for (int i=0; i < v1.size(); i++){
		v2.push_back((Point)v1[i]);
	}
}
void copyTo(Mat &dst,Mat src,Mat mask){
	for (int i = 0; i < dst.rows; i++)
	{
		for (int j = 0; j < dst.cols; j++)
		{
			if (mask.at<uchar>(i,j) != 0) {
				dst.at<Vec3b>(i,j)[0] = src.at<Vec3b>(i,j)[0];
				dst.at<Vec3b>(i,j)[1] = src.at<Vec3b>(i,j)[1];
				dst.at<Vec3b>(i,j)[2] = src.at<Vec3b>(i,j)[2];
			}
		}
	}
}

void dibujarImagen(System::Windows::Forms::PictureBox^ control, cv::Mat& grayImage )
{
	System::Drawing::Bitmap^ bitmap = gcnew System::Drawing::Bitmap(grayImage.cols,grayImage.rows,grayImage.step,System::Drawing::Imaging::PixelFormat::Format32bppArgb,(System::IntPtr) grayImage.data);
	control->Image = bitmap;
	control->Refresh();
}